#include "Measure_Bullet.h"
#include "qdebug.h"
Measure_Bullet::Measure_Bullet()
{

}
int Measure_Bullet::M_Bullet(HObject Img, HTuple &OutPara, HObject &OutImg)
{
    // Local iconic variables
    HObject  Map, ImageMapped, Rect;
    HObject  ImageReduced, Region, ConnectedRegions;
    HObject  SelectedRegions, RegionTrans, Rectangle1;
    HObject  ImageReduced1, Region1, ImageResult1, Edges1;
    HObject  ERegion1, EndPoints1, JuncPoints1, ConnectedRegions1;
    HObject   Rectangle2;
    HObject  ImageReduced2, Region2, ImageResult2, Edges2;
    HObject  Circle, l1;

    // Local control variables

    HTuple  CameraParameters, CameraPose;
    HTuple  Row, Column, Phi, W, H, r1, c1;
    HTuple  line1, line2, MetrologyHandle1, Index1;
    HTuple  Line1, MetrologyHandle2;
    HTuple  Line2, Distance1, Distance2, len1, Convexity;
    HTuple  Max, IndexMax, Min, IndexMin, Row1;
    HTuple  Column1, Radius, StartPhi, EndPhi, PointOrder;
    HTuple  Row2, Col2, R2Length, line3, MetrologyHandle3;
    HTuple  Line3, len2, A1, A2, Len1, Len2, W11, H11, OutPara1, OutPara2,Number;

    Number=0;


    CameraParameters.Clear();
    CameraParameters[0] = 0;
    CameraParameters[1] = -0.940741;
    CameraParameters[2] = 1.14751e-005;
    CameraParameters[3] = 1.14709e-005;
    CameraParameters[4] = 891.283;
    CameraParameters[5] = 943.919;
    CameraParameters[6] = 2000;
    CameraParameters[7] = 1500;
    CameraPose.Clear();
    CameraPose[0] = -0.00709736;
    CameraPose[1] = -0.00755097;
    CameraPose[2] = 1.00222;
    CameraPose[3] = 357.262;
    CameraPose[4] = 359.28;
    CameraPose[5] = 0.546978;
    CameraPose[6] = 0;

    //宽度
    Flag1=true;
    //长度
    Flag2=true;
    //角度1
    Flag3=true;
    //角度2
    Flag4=true;

    //超出上限
    //宽度
    Flag11=true;
    //长度
    Flag22 = true;
    Flag33 = true;
    Flag44 = true;
    OutPara.Clear();
    int flag=0;
    float Angle=0.05;








    //换算比例
    float para1 = m_part.para1;
    //总体补偿
    float para2 = m_part.para2;


    //——————————————————————垂直方向
    //补偿值
    float para3 = m_part.para3;
    //标准值
    float para4 = m_part.para4;
    //上限值
    float para5 = m_part.para5;
    //下限值
    float para6 = m_part.para6;



    //——————————————————————水平方向
    //补偿值
    float para10 = m_part.para10;
    //标准值
    float para11 = m_part.para11;
    //上限值
    float para12 = m_part.para12;
    //下限值
    float para13 = m_part.para13;


    //------------------图像处理

    //粗定定位阈值
    float para17 = m_part.para17;
    //找边向外沿伸长度
    float para18 = m_part.para18;
    //找边sigma
    float para19 = m_part.para19;
    //找边阈值
    float para20 = m_part.para20;


    //角度1补偿值
    float para21 = m_part.para21;
    //角度1标准值
    float para22 = m_part.para22;
    //角度1上限值
    float para23 = m_part.para23;
    //角度1下限值
    float para24 = m_part.para24;


    //角度2补偿值
    float para25 = m_part.para25;
    //角度2标准值
    float para26 = m_part.para26;
    //角度2上限值
    float para27 = m_part.para27;
    //角度2下限值
    float para28 = m_part.para28;


    try
    {
        OutImg=Img;

        GetImageSize(Img, &W11, &H11);
        if ((W11 != 2000) || (H11 != 1500))
        {
            OutPara.Append(0).Append(0).Append(0).Append(0);
            Flag1 = false;
            Flag2 = false;
            Flag3 = false;
            Flag4 = false;

            flag = 2;
            return flag;
        }
        GenRadialDistortionMap(&Map, CameraParameters, CameraParameters, "bilinear");
        MapImage(Img, Map, &ImageMapped);
        GenRectangle1(&Rect, m_part2.x1, m_part2.y1, m_part2.x2, m_part2.y2);

        ReduceDomain(ImageMapped, Rect, &ImageReduced);
        Threshold(ImageReduced, &Region, 0, para17);
        Connection(Region, &ConnectedRegions);

        SelectShape(ConnectedRegions, &SelectedRegions, "area", "and", 10000, 9999999);
        CountObj(ConnectedRegions, &Number);
        if(!(Number.D() == 1))
        {
            OutPara.Append(0).Append(0).Append(0).Append(0);
            Flag1 = false;
            Flag2 = false;
            Flag3 = false;
            Flag4 = false;

            flag = -1;
            return flag;
        }



//        SelectShapeStd(ConnectedRegions, &SelectedRegions, "max_area", 70);
        ShapeTrans(SelectedRegions, &RegionTrans, "rectangle2");
        SmallestRectangle2(RegionTrans, &Row, &Column, &Phi, &W, &H);


        //高度
        GenRectangle2(&Rectangle1, Row, Column, Phi, W/3, H+60);

        ReduceDomain(ImageMapped, Rectangle1, &ImageReduced1);
        Threshold(ImageReduced1, &Region1, 0, para17);
        Connection(Region1, &Region1);



        SelectShapeStd(Region1, &Region1, "max_area", 70);



        DilationRectangle1(Region1, &Region1, para18, para18);



        ReduceDomain(ImageMapped, Region1, &ImageResult1);
        EdgesSubPix(ImageResult1, &Edges1, "canny", 3, 20, 40);
        SelectContoursXld(Edges1, &Edges1, "contour_length", 100, 20000, -0.5, 0.5);
        UnionAdjacentContoursXld(Edges1, &Edges1, 10, 1, "attr_keep");
        GenRegionContourXld(Edges1, &ERegion1, "filled");
        SelectShape(ERegion1, &ERegion1, "width", "and", ((2*W)/3)-30, ((2*W)/3)+30);
        JunctionsSkeleton(ERegion1, &EndPoints1, &JuncPoints1);
        Connection(EndPoints1, &ConnectedRegions1);
        RegionFeatures(ConnectedRegions1, "row", &r1);
        RegionFeatures(ConnectedRegions1, "column", &c1);
        line1.Clear();
        line1.Append(HTuple(r1[0]));
        line1.Append(HTuple(c1[0]));
        line1.Append(HTuple(r1[1]));
        line1.Append(HTuple(c1[1]));
        line2.Clear();
        line2.Append(HTuple(r1[2]));
        line2.Append(HTuple(c1[2]));
        line2.Append(HTuple(r1[3]));
        line2.Append(HTuple(c1[3]));

        CreateMetrologyModel(&MetrologyHandle1);
        AddMetrologyObjectLineMeasure(MetrologyHandle1, HTuple(line1[0]), HTuple(line1[1]),
                HTuple(line1[2]), HTuple(line1[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                &Index1);
        ApplyMetrologyModel(ImageMapped, MetrologyHandle1);
        GetMetrologyObjectResult(MetrologyHandle1, "all", "all", "result_type", "all_param",
                                 &Line1);
        ClearMetrologyModel(MetrologyHandle1);



        CreateMetrologyModel(&MetrologyHandle2);
        AddMetrologyObjectLineMeasure(MetrologyHandle2, HTuple(line2[0]), HTuple(line2[1]),
                HTuple(line2[2]), HTuple(line2[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                &Index1);
        ApplyMetrologyModel(ImageMapped, MetrologyHandle2);
        GetMetrologyObjectResult(MetrologyHandle2, "all", "all", "result_type", "all_param",
                                 &Line2);
        ClearMetrologyModel(MetrologyHandle2);
        DistancePl(HTuple(Line1[0]), HTuple(Line1[1]), HTuple(Line2[0]), HTuple(Line2[1]),
                HTuple(Line2[2]), HTuple(Line2[3]), &Distance1);
        DistancePl(HTuple(Line1[2]), HTuple(Line1[3]), HTuple(Line2[0]), HTuple(Line2[1]),
                HTuple(Line2[2]), HTuple(Line2[3]), &Distance2);

        //
        len1 = (Distance1+Distance2)/2;
        Len1 = len1*para1+para2+0.018618;


        if (0 != (HTuple(Phi<(PI/2)).TupleOr(Phi>((PI*3)/4))))
        {
            Phi += PI/2;
        }
        else
        {
            Phi = Phi;
        }


        //长度
        GenRectangle2(&Rectangle2, Row, Column, Phi, H/2, W+60);
        ReduceDomain(ImageMapped, Rectangle2, &ImageReduced2);
        Threshold(ImageReduced2, &Region2, 0, para17);
        Connection(Region2, &Region2);
        SelectShapeStd(Region2, &Region2, "max_area", 70);
        DilationRectangle1(Region2, &Region2, para18, para18);
        ReduceDomain(ImageMapped, Region2, &ImageResult2);
        EdgesSubPix(ImageResult2, &Edges2, "canny", 2, 20, 40);
        SelectContoursXld(Edges2, &Edges2, "contour_length", 100, 20000, -0.5, 0.5);

        ConvexityXld(Edges2, &Convexity);
        TupleMax(Convexity, &Max);
        TupleFind(Convexity, Max, &IndexMax);
        TupleMin(Convexity, &Min);
        TupleFind(Convexity, Min, &IndexMin);

        SelectObj(Edges2, &Circle, IndexMax+1);
        FitCircleContourXld(Circle, "algebraic", -1, 0, 0, 3, 2, &Row1, &Column1,
                            &Radius, &StartPhi, &EndPhi, &PointOrder);


        SelectObj(Edges2, &l1, IndexMin+1);
        GetContourXld(l1, &Row2, &Col2);
        TupleLength(Row2, &R2Length);

        line3.Clear();
        line3.Append(HTuple(Row2[0]));
        line3.Append(HTuple(Col2[0]));
        line3.Append(HTuple(Row2[R2Length-1]));
        line3.Append(HTuple(Col2[R2Length-1]));

        CreateMetrologyModel(&MetrologyHandle3);
        AddMetrologyObjectLineMeasure(MetrologyHandle3, HTuple(line3[0]), HTuple(line3[1]),
                HTuple(line3[2]), HTuple(line3[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                &Index1);
        ApplyMetrologyModel(ImageMapped, MetrologyHandle3);
        GetMetrologyObjectResult(MetrologyHandle3, "all", "all", "result_type", "all_param",
                                 &Line3);
        ClearMetrologyModel(MetrologyHandle3);
        DistancePl(Row1, Column1, HTuple(Line3[0]), HTuple(Line3[1]), HTuple(Line3[2]),
                HTuple(Line3[3]), &Distance2);


        Len2 = (Radius+Distance2)*para1+para2+0.0234;






        if (0 != (Phi>0))
        {
            AngleLl(HTuple(Line1[0]), HTuple(Line1[1]), HTuple(Line1[2]), HTuple(Line1[3]),
                    HTuple(Line3[2]), HTuple(Line3[3]), HTuple(Line3[0]), HTuple(Line3[1]),
                    &A1);


            AngleLl(HTuple(Line2[0]), HTuple(Line2[1]), HTuple(Line2[2]), HTuple(Line2[3]),
                    HTuple(Line3[0]), HTuple(Line3[1]), HTuple(Line3[2]), HTuple(Line3[3]),
                    &A2);
        }
        else
        {
            AngleLl(HTuple(Line1[0]), HTuple(Line1[1]), HTuple(Line1[2]), HTuple(Line1[3]),
                    HTuple(Line3[0]), HTuple(Line3[1]), HTuple(Line3[2]), HTuple(Line3[3]),
                    &A1);

            AngleLl(HTuple(Line2[0]), HTuple(Line2[1]), HTuple(Line2[2]), HTuple(Line2[3]),
                    HTuple(Line3[2]), HTuple(Line3[3]), HTuple(Line3[0]), HTuple(Line3[1]),
                    &A2);
        }

        A1 = A1 * 180 / PI;
        A2 = A2 * 180 / PI;

        TupleAbs(A1, &A1);
        TupleAbs(A2, &A2);

        A1 = A1 + para21;
        A2 = A2 + para25;

        if (!((A1<(para22 + para23-Angle)) && (A1>(para22 - para24+Angle))))
        {
            if (A1 < (para22 - para24))
            {
                Flag33 = false;
            }
            Flag3 = false;
            flag = 1;
        }
        if (!((A2<(para26 + para27-Angle)) && (A2>(para26 - para28+Angle))))
        {
            if (A2 < (para26 - para28))
            {
                Flag44 = false;
            }
            Flag4 = false;
            flag = 1;
        }
        if((Len1>(para4-0.1))&&((Len1>(para4+0.1))&&((Len2)>(para11 - 0.1))&&(Len2)>(para11 + 0.1)))
        {
            if (!(((Len1) > (para4 - para6)) && ((Len1)<(para4 + para5))))
            {
                if ((Len1)<(para4 - para6))
                {
                    Flag11 = false;
                }
                Flag1 = false;
                flag = 1;
            }
            if (!(((Len2)>(para11 - para13)) && ((Len2) < (para11 + para12))))
            {
                if ((Len2) < (para11 - para13))
                {
                    Flag22 = false;
                }
                Flag2 = false;
                flag = 1;
            }

        }
        else
        {
            Flag1 = false;
            Flag2 = false;
            flag = -1;
        }
        TupleConcat(Len1, Len2, &OutPara1);

        TupleConcat(A1, A2, &OutPara2);
        TupleConcat(OutPara1, OutPara2, &OutPara);

        //        qDebug()<<OutPara[0].D()<<OutPara[1].D()<<OutPara[2].D()<<OutPara[3].D();
        return flag;
    }
    catch(HException ex)
    {
        OutPara.Append(0).Append(0).Append(0).Append(0);
        //宽度
        Flag1=false;
        //长度
        Flag2=false;
        //角度1
        Flag3=false;
        //角度2
        Flag4=false;

        flag=2;
        return flag;
    }


}

